import launch
import launch.event_handlers
import launch.launch_description_sources
import launch_ros
from ament_index_python.packages import get_package_share_directory
import os
import launch_ros.parameter_descriptions

def generate_launch_description():
    urdf_packge_path=get_package_share_directory('firstrobot_description')
    default_urdf_path=os.path.join(urdf_packge_path,'urdf/fishrobot','fishrobot.urdf.xacro')
    default_gazebo_world_path=os.path.join(urdf_packge_path,'world','custom_room1.world')
    
    #声明一个urdf目录的参数方便修改
    action_declare_arg_mode_path=launch.actions.DeclareLaunchArgument(
        name='model',default_value=str(default_urdf_path),description='加载文件路径'
    )


    # 通过文件路径获取内容，并转换成参数对象，并传入robot_state_publisher
    subscription_command_result = launch.substitutions.Command(
    ['xacro ', launch.substitutions.LaunchConfiguration('model')]# 禁用shell模式以避免参数合并
    )
    # with open(default_urdf_path, 'r') as f:
    #     urdf_content = f.read()
    # robot_description_value = launch_ros.parameter_descriptions.ParameterValue(urdf_content, value_type=str)

    robot_description_value=launch_ros.parameter_descriptions.ParameterValue(
        subscription_command_result,value_type=str)
    print(robot_description_value)


    action_robot_state_publisher=launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}],
    )

    action_launch_gazebo=launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource(
            [get_package_share_directory('gazebo_ros'),'/launch','/gazebo.launch.py']
        ),
        launch_arguments=[('world',default_gazebo_world_path),('verbose','true')],
    )
    # action_joint_state_publisher=launch_ros.actions.Node(
    # package='joint_state_publisher',
    # executable='joint_state_publisher',
      
    # )

    # action_rviz2_node=launch_ros.actions.Node(
    #     package='rviz2',
    #     executable='rviz2',
    #     arguments=['-d',default_rviz2config_path]
    # )
    action_spawn_entity=launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity','fishbot','-topic','/robot_description'],
         output='screen',
    )

    action_load_joint_states_controller=launch.actions.ExecuteProcess(
        cmd='ros2 control load_controller fishbot_joint_state_broadcaster --set-state active'.split(' '),
        output='screen'
    )
    # action_load_effort_controller=launch.actions.ExecuteProcess(
    #     cmd='ros2 control load_controller fishbot_effort_controller --set-state active'.split(' '),
    #     output='screen'
    # )
    action_load_diff_drive_controller=launch.actions.ExecuteProcess(
        cmd='ros2 control load_controller fishbot_diff_drive_controller --set-state active'.split(' '),
        output='screen'
    )

    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_robot_state_publisher,
        action_launch_gazebo,
        action_spawn_entity,
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_spawn_entity,
                on_exit=[action_load_joint_states_controller],
            )
        ),
                launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_load_joint_states_controller,
                on_exit=[action_load_diff_drive_controller],
            )
        ),
        # action_joint_state_publisher,
        # action_rviz2_node,


    ])